﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace ImageDefine
{
    public static class MachineVisionToolKit
    {
        static public void GetTargetPosForToolAndAngle(double dCenterPosX, double dCenterPosY, double dCenterPosU,
                                               double dHandX, double dHandY, double dHandU,
                                               double dObjectX, double dObjectY, double dObjectU,
                                               out double dTargetX, out double dTargetY, out double dTargetU)
        {
            dTargetX = 0;
            dTargetY = 0;
            dTargetU = 0;

            double dCenterX = dCenterPosX;
            double dCenterY = dCenterPosY;
            double dCenterU = dCenterPosU;
            double dAngleOffet = dObjectU - dHandU;
            if (dAngleOffet > 180)
            {
                dAngleOffet = dAngleOffet - 360;
            }
            else if (dAngleOffet < -180)
            {
                dAngleOffet = dAngleOffet + 360;
            }
            double dOrgX = dHandX;
            double dOrgY = dHandY;

            double dNewX = 0;
            double dNewY = 0;



            double R = Math.Sqrt((dCenterX - dOrgX) * (dCenterX - dOrgX) + (dCenterY - dOrgY) * (dCenterY - dOrgY));
            double tanAngleBeforePa = Math.Atan2(dOrgY - dCenterY, dOrgX - dCenterX);
            double tanAngleBefore = tanAngleBeforePa * 180.0 / Math.PI;
            double tanAngleAfter = tanAngleBefore + dAngleOffet;

            dNewX = dCenterX + R * Math.Cos(tanAngleAfter * Math.PI / 180.0);
            dNewY = dCenterY + R * Math.Sin(tanAngleAfter * Math.PI / 180.0);


            dTargetX = dCenterX + (dObjectX - dNewX);
            dTargetY = dCenterY + (dObjectY - dNewY);
            dTargetU = dCenterU + dAngleOffet;
        }
    }
}
